# -*- coding: utf-8 -*-
# @copyright Copyright (c) 2022 OnMicro Corp.
# @brief     SNC Field Oriented Control algorithm test.
# @author    wei.lu@onmicro.com.cn
# @license   SPDX-License-Identifier: Apache-2.0

import wave
import pylab as plt
import matplotlib.patches as pat
import numpy as np
import scipy.stats as stats

# Based on https://www.zhihu.com/question/322254277/answer/669209216
def rect_square_wave(d, origin=0, duty_ratio=0.5, period=1, low=0, high=1):
    """
    Parameters
    ----------
    origin: number
        x 起始位置
    duty_ratio: number
        一个周期中duty占比
    period: number
        x轴中一个周期的长度
    low: number
        y轴上方波low value
    high: number
        y轴上方波high value

    Returns
    """
    start = origin
    d.append((start, low))
    pre = start + period * (1.0-duty_ratio)/2
    d.append((pre, low))
    d.append((pre, high))
    post = pre + period * duty_ratio
    d.append((post, high))
    d.append((post, low))
    end = post + period * (1.0-duty_ratio)/2
    d.append((end, low))

    return d

if __name__ == "__main__":
    import os,sys
    if not (len(sys.argv)>1 and os.path.isfile(sys.argv[1])):
        print('usage: {} sin.txt'.format(sys.argv[0]))
        sys.exit(1)

    angle = []
    alpha = []
    beta = []
    a = []
    b = []
    c = []
    fa = []
    fb = []
    fc = []
    abc = []
    duty1 = []
    duty2 = []
    duty3 = []

    # Read the output of snc_utest.c and plot its sin.
    fh = open(sys.argv[1], "r")
    for row in fh:
        row = row.split(' ')
        angle.append(row[0])
        alpha.append(int(row[1]))
        beta.append(int(row[2]))
        # Clarke inv transform (var.) in float point version
        fa.append(int(row[2])*1.0)
        fb.append((int(row[2])*-0.5 + 0.8660254*int(row[1]))/1)
        fc.append((int(row[2])*-0.5 - 0.8660254*int(row[1]))/1)
        # Clarke inv transform (var.) in fix point Q15 version
        a.append(int(row[3]))
        b.append(int(row[4]))
        c.append(int(row[5]))
        #print("b=%d %f; c=%d %f\n" % (int(row[4]), int(row[5]), int(row[2])*-0.5 + 0.8660254*int(row[1]), int(row[2])*-0.5 - 0.8660254*int(row[1])))
        #abc.append(int(row[3])+int(row[4])+int(row[5]))
        duty1.append(int(row[6]))
        duty2.append(int(row[7]))
        duty3.append(int(row[8]))
    fh.close()

    plt.figure()
    plt.xlabel('angle (radius)', fontsize = 12)
    plt.ylabel('voltage', fontsize = 12)
    plt.title('alpha-beta (90-diff), a-b-c (120-diff) voltage waveform', fontsize = 20)

    plt.yticks(alpha)
    plt.plot(angle, alpha, marker = ',', color = 'r', label = 'Valpha', linewidth=6.0)
    plt.yticks(beta)
    plt.plot(angle, beta, marker = ',', color = 'g', label = 'Vbeta', linewidth=6.0)
    plt.yticks(a)
    plt.plot(angle, a, marker = '.', color = 'r', label = 'Va')
    plt.yticks(b)
    plt.plot(angle, b, marker = '.', color = 'g', label = 'Vb')
    plt.yticks(c)
    plt.plot(angle, c, marker = '.', color = 'b', label = 'Vc')
    plt.yticks(fa)
    plt.plot(angle, fa, marker = '.', color = 'r', label = 'Va-f32')
    plt.yticks(fb)
    plt.plot(angle, fb, marker = '*', color = 'g', label = 'Vb-f32')
    plt.yticks(fc)
    plt.plot(angle, fc, marker = '*', color = 'b', label = 'Vc-f32')
    #plt.yticks(abc)
    #plt.plot(angle, abc, marker = '*', color = 'black', label = 'Vabc')
    # Level 0
    plt.axhline(0, color = 'g')
    # Level max
    plt.axhline(0x7fff, color = 'g')
    plt.grid()
    plt.legend()

    plt.figure()
    plt.title('3-phase PWM waveform', fontsize = 20)
    pwm1 = []
    pwm2 = []
    pwm3 = []
    idx = 0
    while idx < min(128,len(duty1)):
        rect_square_wave(pwm1, origin=idx, duty_ratio=duty1[idx]/0x7fff, period=1, low=3.0, high=4.0)
        rect_square_wave(pwm2, origin=idx, duty_ratio=duty2[idx]/0x7fff, period=1, low=1.5, high=2.5)
        rect_square_wave(pwm3, origin=idx, duty_ratio=duty3[idx]/0x7fff)
        idx += 1
    pwm = np.array(pwm1)
    plt.plot(pwm[:,0], pwm[:,1], color = 'r', label = 'pwm1')
    pwm = np.array(pwm2)
    plt.plot(pwm[:,0], pwm[:,1], color = 'g', label = 'pwm2')
    pwm = np.array(pwm3)
    plt.plot(pwm[:,0], pwm[:,1], color = 'b', label = 'pwm3')
    plt.axhline(4.5, color = 'g')
    plt.axhline(-0.5, color = 'g')
    plt.xticks(np.arange(start=1, stop=idx, step=1))
    plt.grid()
    plt.legend()

    plt.show()
